/**
 ******************************************************************************
 * @file    mini_dog.c
 * @author  GEEKROS,  site:www.geekros.com
 ******************************************************************************
 */

#include "mini_dog.h"

Lite_Dog_Struct Lite_Dog;

/*******************************************************************************
  * @funtion      : Get_Lite_Dog_Data_Address  
  * @description  : 返回机器狗结构体地址
  * @param         {*}
  * @return        {*}
  *******************************************************************************/
Lite_Dog_Struct* Get_Lite_Dog_Data_Address(void)
{
	return &Lite_Dog;
}

/*******************************************************************************
  * @funtion      : Mini_Dog_Init  
  * @description  : 机器狗数据初始化
  * @param         {*}
  * @return        {*}
  *******************************************************************************/
void Mini_Dog_Init(void)
{
	
	// 打开扩展板供电电源接口
	Power_24v_State(0, "on");

	//设置舵机串口波特率
	Wifi_Init(115200);
	
	// 设置舵机串口波特率
	Servo_Init(1000000);
	
	// 设置舵机类型
	Servo_Type(0, SERVO_SCS);
	
	// 设置电池低电压报警阀值，低电量时开发板红色LED灯亮起
	Adc_Low_Voltage(11.4);

	// 初始模式设置
	Lite_Dog.Dog_Mode = Dog_Order_Control_Mode;
	Lite_Dog.Dog_Control_Data.Dog_Set_Move_Data.Dog_Move_Mode = Stop_M;
	Lite_Dog.Dog_Control_Data.Dog_Set_Move_Data.Dog_Sport_Mode = Trot_M;
	
	Lite_Dog.Dog_Control_Data.Dog_Order_Mode_Data = Get_Order_Mode_Address();
	Dog_Order_Mode_Init();
	Lite_Dog.Dog_Control_Data.Dog_Remote_Mode = Dog_RM_Att_Control_Mode;
	Lite_Dog.Dog_Control_Data.Dog_Set_Move_Data.All_Leg_Move_Time = 1000;
	Lite_Dog.Dog_Control_Data.Dog_Set_Move_Data.Leg_Move_Duty = 0.5;
	Lite_Dog.Dog_Control_Data.Dog_Set_Move_Data.Single_Leg_Move_Time = 500;

	// 设置机器狗基本配置
	Lite_Dog.Dog_Config_Data.servo_len = Lite_Dog_Len;
	Lite_Dog.Dog_Config_Data.servo_angle_range = 360.f; //舵机转动角度范围
	Lite_Dog.Dog_Config_Data.body_length = 157.0f; //身体前后舵机间距
	Lite_Dog.Dog_Config_Data.body_width = 108.0f; // 身体内侧舵机间距
	Lite_Dog.Dog_Config_Data.foot_width = 116.0f; // 身体两侧足部间距
	Lite_Dog.Dog_Config_Data.butt_length = 27.0028f;
	Lite_Dog.Dog_Config_Data.thigh_length = 54.4117f; // 大腿长度
	Lite_Dog.Dog_Config_Data.calf_length = 58.6911f; // 小腿成都
	Lite_Dog.Dog_Config_Data.joint_offset = 310; //关节偏移量

	Lite_Dog.Dog_Config_Data.pitch_error = Dog_Pit_Error;
	Lite_Dog.Dog_Config_Data.roll_error = Dog_Roll_Error;
	Lite_Dog.Dog_Config_Data.yaw_error = Dog_Yaw_Error;

	Lite_Dog.Dog_Config_Data.pitch_max = 20.00f;
	Lite_Dog.Dog_Config_Data.roll_max = 20.00f;
	Lite_Dog.Dog_Config_Data.yaw_max = 4.00f;

	Lite_Dog.Dog_Config_Data.pitch_add = 4.80f;
	Lite_Dog.Dog_Config_Data.roll_add = 3.60f;
	Lite_Dog.Dog_Config_Data.yaw_add = 0.40f;

	Lite_Dog.Dog_Config_Data.x_max = 67.00f;
	Lite_Dog.Dog_Config_Data.x_min = -40.00f;
	Lite_Dog.Dog_Config_Data.y_max = 30.00f;
	Lite_Dog.Dog_Config_Data.y_min = -30.00f;
	Lite_Dog.Dog_Config_Data.z_max = 90.00f;
	Lite_Dog.Dog_Config_Data.z_min = 50.00f;

	// MPU数据初始化
	Lite_Dog.Dog_State_Data.Dog_Mpu_Data = Return_Mpu_Address(); 
	Lite_Dog.Dog_State_Data.Dog_Body_Now_Att = Body_All_Up;
	
	// 姿态初始化
	Lite_Dog.Target_Attitude.x = Dog_Init_Gravity_X;
	Lite_Dog.Target_Attitude.y = Dog_Init_Gravity_Y;
	Lite_Dog.Target_Attitude.z = Dog_Init_Height;

	// 保存控制值地址
	Lite_Dog.Dog_State_Data.Dog_Body_Set_Cood[0] = Lite_Dog.Dog_LF_Leg_Data.Set_Leg_Loc;
	Lite_Dog.Dog_State_Data.Dog_Body_Set_Cood[1] = Lite_Dog.Dog_RF_Leg_Data.Set_Leg_Loc;
	Lite_Dog.Dog_State_Data.Dog_Body_Set_Cood[2] = Lite_Dog.Dog_LB_Leg_Data.Set_Leg_Loc;
	Lite_Dog.Dog_State_Data.Dog_Body_Set_Cood[3] = Lite_Dog.Dog_RB_Leg_Data.Set_Leg_Loc;

	// 保存当前值地址
	Lite_Dog.Dog_State_Data.Dog_Body_Now_Cood[0] = Lite_Dog.Dog_LF_Leg_Data.Now_Leg_Loc;
	Lite_Dog.Dog_State_Data.Dog_Body_Now_Cood[1] = Lite_Dog.Dog_RF_Leg_Data.Now_Leg_Loc;
	Lite_Dog.Dog_State_Data.Dog_Body_Now_Cood[2] = Lite_Dog.Dog_LB_Leg_Data.Now_Leg_Loc;
	Lite_Dog.Dog_State_Data.Dog_Body_Now_Cood[3] = Lite_Dog.Dog_RB_Leg_Data.Now_Leg_Loc;
	
	// 保存遥控器数据
	Lite_Dog.Dog_Control_Data.Dog_Remote_Control = Return_Rocker_Address();
	Lite_Dog.Dog_Control_Data.Dog_Set_Move_Data.Dog_Config_Data = &Lite_Dog.Dog_Config_Data;
	
	// 
	Lite_Dog.Dog_LF_Leg_Data.Leg_Loc = Dog_LF_Leg;
	Lite_Dog.Dog_RF_Leg_Data.Leg_Loc = Dog_RF_Leg;
	Lite_Dog.Dog_LB_Leg_Data.Leg_Loc = Dog_LB_Leg;
	Lite_Dog.Dog_RB_Leg_Data.Leg_Loc = Dog_RB_Leg;
	
	// 身体舵机数据
	Lite_Dog.Dog_LF_Leg_Data.Leg_Motor_Message[0] = Return_Servo_Address(1);
	Lite_Dog.Dog_RF_Leg_Data.Leg_Motor_Message[0] = Return_Servo_Address(2);
	Lite_Dog.Dog_LB_Leg_Data.Leg_Motor_Message[0] = Return_Servo_Address(3);
	Lite_Dog.Dog_RB_Leg_Data.Leg_Motor_Message[0] = Return_Servo_Address(4);
	
	// 大腿舵机数据
	Lite_Dog.Dog_LF_Leg_Data.Leg_Motor_Message[1] = Return_Servo_Address(5);
	Lite_Dog.Dog_RF_Leg_Data.Leg_Motor_Message[1] = Return_Servo_Address(6);
	Lite_Dog.Dog_LB_Leg_Data.Leg_Motor_Message[1] = Return_Servo_Address(7);
	Lite_Dog.Dog_RB_Leg_Data.Leg_Motor_Message[1] = Return_Servo_Address(8);
	
	// 小腿舵机数据
	Lite_Dog.Dog_LF_Leg_Data.Leg_Motor_Message[2] = Return_Servo_Address(9);
	Lite_Dog.Dog_RF_Leg_Data.Leg_Motor_Message[2] = Return_Servo_Address(10);
	Lite_Dog.Dog_LB_Leg_Data.Leg_Motor_Message[2] = Return_Servo_Address(11);
	Lite_Dog.Dog_RB_Leg_Data.Leg_Motor_Message[2] = Return_Servo_Address(12);
	
}

/*******************************************************************************
  * @funtion      : Mini_Dog_Task  
  * @description  : 机器狗任务入口
  * @param         {*}
  * @return        {*}
  *******************************************************************************/
Lite_Dog_Mode_Enum Dog_Mode_Last;//保存上一次模式值
void Mini_Dog_Task(void)
{
	// 检测flash数据读取是否合法
	if(Lite_Dog.Dog_State == Dog_OffsetData_Loss)
	{
		if(Lite_Dog_Read_Flash_Data(&Lite_Dog) == 1)
		{
			Lite_Dog.Dog_State = Dog_OK;
		}
	}
	
	// 状态更新
	Lite_Dog_Attitude_Update(&Lite_Dog.Dog_State_Data);
	
	// 倒地自恢复(可注释)
	// Dog_Recovery_Event(&Lite_Dog);

	// 获取舵机参数(可注释)
	// Lite_Dog_Servo_To_Data(&Lite_Dog);
	
	// 初始化模式
	if(Lite_Dog.Dog_Mode == Dog_Init_Mode)
	{
		Lite_Dog_Init_Motion_Set(&Lite_Dog);
		if(Lite_Dog.Dog_State == Dog_Done)
		{
			Lite_Dog.Dog_Mode = Dog_Order_Control_Mode;
		}
	}
	
	// 遥控器模式
	if(Lite_Dog.Dog_Mode == Dog_Remote_Control_Mode)
	{
		// 遥控器模式设置
		Dog_Remote_Control_Mode_Set(&Lite_Dog.Dog_Control_Data);
		// 遥控器模式更新
		Dog_Remote_Control_Mode_Update(&Lite_Dog);
	}
	
	// 指令模式
	if(Lite_Dog.Dog_Mode == Dog_Order_Control_Mode)
	{
		if(Dog_Mode_Last != Dog_Order_Control_Mode)
		{
			// 如果之前不是指令模式，初始化指令模式相关数据值
			Dog_Order_Mode_Init();
		}
		// 指令模式更新
		Dog_Order_Motion_Data_Update(&Lite_Dog);
	}
	
	// 无力模式
	if(Lite_Dog.Dog_Mode == Dog_PowerLess_Mode)
	{
		Lite_Dog.Dog_State = Dog_PowerLess;
		Lite_Dog_Set_PowerLess();
	}
	
	// 判断是否是处于可以控制状态
	if(Lite_Dog.Dog_State == Dog_OK)
	{
		// 坐标解算，坐标-角度
		Lite_Dog_Coordinate_To_Angle(&Lite_Dog,Lite_Dog.Dog_Set_XY_Coordinate[0],Lite_Dog.Dog_Set_XY_Coordinate[1],Lite_Dog.Dog_Set_XY_Coordinate[2]);

		// 舵机值发送,角度-舵机值
		Lite_Dog_Data_To_Servo(&Lite_Dog ,0 ,0);
	}
	
	delay_ms(Dog_Delay_Time_Set);

	Dog_Mode_Last = Lite_Dog.Dog_Mode;
}

/*******************************************************************************
  * @funtion      : Lite_Dog_Init_Motion_Set  
  * @description  : 机器狗初始化动作设置
  * @param         {Lite_Dog_Struct*} Dog_Init 机器狗数据结构体地址
  * @return        {*}
  *******************************************************************************/
void Lite_Dog_Init_Motion_Set(Lite_Dog_Struct* Dog_Init)
{



}

/*******************************************************************************
  * @funtion      : Lite_Dog_Change_Mode  
  * @description  : 机器狗模式切换
  * @param         {Lite_Dog_Mode_Enum Dog_Mode_Change} 模式
  * @return        {*}
  *******************************************************************************/
void Lite_Dog_Change_Mode(Lite_Dog_Mode_Enum Dog_Mode_Change)
{
	Lite_Dog.Dog_Mode = Dog_Mode_Change;
}
